function [B] = limitposdefmatdiag(A, diagMin, diagMax) %#codegen
%LIMITPOSDEFMATDIAG 限制非负定矩阵对角线元素在指定范围内
%
% Input Arguments:
% A: 二维非负定对称矩阵
% PDiagMin: m*1列向量，各元素为A对角线元素对应的最小值
% PDiagMax: m*1列向量，各元素为A对角线元素对应的最大值
%
% Output Arguments:
% B: 尺寸与A相同的非负定对称矩阵，对角线元素不小于diagMin，不大于diagMax
%
% References:
% [1] 理论文档 版本号1.0 章“卡尔曼滤波” 节1.4.1“发散的抑制”

B = A;
m = size(B, 1);
J = eye(m);
for i=1:m
    if B(i, i) < diagMin(i, 1)
        B(i, i) = diagMin(i, 1); % REF1式12.118
    end
    % REF1式12.124
    if B(i, i) > diagMax(i, 1)
        J(i, i) = sqrt(diagMax(i, 1)/B(i, i));
    else
        J(i, i) = 1;
    end
    B = J * B * J'; % REF1式12.122
end
end